///*
//STM32部分只需要听从K210的命令，完美执行，完了之后及时反馈即可

//todo list：
//1.编码器测到的速度累加起来的位移 = 0.7649459121011438 * 实际位移。该公式需要继续测量
//2.尽管加入了PID算法，试图让车子走直线，但做得还是不够好。应该修正一下各参数，力求达到更好的稳定性
//3.精进角度阈值

//记得每次测试前先校准速度比例，尽管加入了PID算法来使得其尽量走直线，但如果一开始比例就十分得当的话收敛到理想配比的速度会更快。测试功能的话可以给接收数据包
//的数组Serial_RxPacket赋相应初值，也可以连上K210，顺便看它们的串口功能是否正常。
//*/
#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "OLED.h"
#include "Serial.h"
#include "Key.h"
#include "Motor.h"
#include "MPU6050.h"
#include "Timer.h"
#include "Encoder.h"
#include "PID.h"
#include "No_Mcu_Ganv_Grayscale_Sensor_Config.h"
#include "gpio.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>

#define DISPLACEMENT_CHANGING_RATE 0.7423888182973331//实际位移乘上该比例即可得到所需的、虚拟的目标位移
#define GZ_CORRECTION_COEFFICIENT 10//陀螺仪在静态情况下测量出的角速度在+1~+2之间波动，因此在使用该值时需要减去该修正系数

bool isTIM2_IRQHandler = true;
int cur_speed_left = 0;
int cur_speed_right = 0;

int8_t Speed1 = 100;
int8_t Speed2 = 100;//左、右两轮应达到的转速。Speed1代表右轮
int16_t Speed;//某个轮子的实际位移。因为车辆在前进时只会直走，所以只测量其中一个轮子的速度即可反映整辆车的速度，进而求得位移。
//行比较，只要超过了就会停下。
float angle = 0.0f;//当前角度。越大代表越向左。
float target_angle = 0.0f;//目标角度。
int16_t GZ;            //定义绕Z轴的角速度。MPU6050所能测量的其余物理量暂时看来没有用处

unsigned char rx_buff[256]={0};
unsigned short Anolog[8]={0};
unsigned short white[8]={1330, 1850, 2310, 2700, 2680,2350, 2530, 2600};
unsigned short black[8]={460, 560, 980, 1480, 910, 650, 920, 1050};
unsigned short Normal[8];


//以居于中间的传感器的状态为低位，越往边缘值越高，将左右各四个传感器的状态转化为数字，进行直走时是否偏航的判断。
uint8_t left_digital = 0;
uint8_t right_digital = 0;



enum CarState { RUN, STOP } CurCarState;



unsigned char GetDigtalValue(No_MCU_Sensor* sensor, int GetValueFrequency)
{
    int HalfCount = GetValueFrequency / 2;
    unsigned char DigtalValue = 0;
    char GetCount[8] = { 0 };
    while(GetValueFrequency--)
    {
        //无时基传感器常规任务，包含模拟量，数字量，归一化量
        No_Mcu_Ganv_Sensor_Task_Without_tick(sensor);
        //获取传感器数字量结果(只有当有黑白值传入进去了之后才会有这个值！！)
        unsigned char CurDigtal = Get_Digtal_For_User(sensor);
        for(int count = 0; count < 8; count++)
            GetCount[count] += ((CurDigtal >> count) & 0x1);
        //经典版理论性能1khz，只需要delay1ms，青春版100hz，需要delay10ms，否则不能正常使用
        Delay_ms(1);
    }
    
    for(int count = 0; count < 8; count++)
    {
        if(GetCount[count] >= HalfCount)
            DigtalValue |= (0x1 << count);
        else
            DigtalValue |= (0x0 << count);
    }

    return DigtalValue;
}


enum MoveWay
{
	STRAIGHT_FORWARD,
	STRAIGHT_BACKWARD,
	LEFT_FORWARD,
	LEFT_BACKWARD,
	RIGHT_FORWARD,
	RIGHT_BACKWARD
};

void ModifyMoveSpeed(int start_speed, int end_speed, bool is_left)
{
	int accelerate = 1;
	int delay_time = 2;
	
	if(end_speed - start_speed > 0)
	{
		while(start_speed < end_speed)
		{
			start_speed += accelerate;

			if(is_left) 
				Motor_SetSpeed_4(start_speed);
			else	
				Motor_SetSpeed_1(start_speed);
			
			Delay_ms(delay_time);
		}
	}
	
	else if(end_speed - start_speed < 0)
	{
		while(start_speed > end_speed)
		{
			start_speed -= accelerate;

			if(is_left) 
				Motor_SetSpeed_4(start_speed);	
			else	
				Motor_SetSpeed_1(start_speed);

			
			Delay_ms(delay_time);
		}
	}
		
	if(is_left)
		cur_speed_left = end_speed;
	else
		cur_speed_right = end_speed;
	
}


void ModifyMoveWay(enum MoveWay way)
{
	bool RightAngleFlag = true; // 为true表示顺时针为正，为false表示逆时针为正

	if(STRAIGHT_FORWARD == way)
	{
		ModifyMoveSpeed(cur_speed_left, 50, true); // 动左轮
		ModifyMoveSpeed(cur_speed_right, 50, false); // 动右轮
	}
	
	else if(STRAIGHT_BACKWARD == way)
	{
		ModifyMoveSpeed(cur_speed_left, -50, true);
		ModifyMoveSpeed(cur_speed_right, -50, false);
	}
	
	else if(RIGHT_FORWARD == way)
	{
		ModifyMoveSpeed(cur_speed_left, 50, true);
		ModifyMoveSpeed(cur_speed_right, 0, false);
		angle = 0;
		while(1)
		{
			Delay_ms(10);
			if((0 >= 90 && RightAngleFlag) || (angle <= -90 && !RightAngleFlag))
			{
				ModifyMoveSpeed(cur_speed_left, 0, true);
				ModifyMoveSpeed(cur_speed_right, 0, false);
				break;
			}
		}
	}
	
	else if(LEFT_FORWARD == way)
	{
		ModifyMoveSpeed(cur_speed_left, 0, true);
		ModifyMoveSpeed(cur_speed_right, 50, false);
		angle = 0;
		while(1)
		{
			Delay_ms(10);
			if(angle <= -90 && RightAngleFlag)
			{
				ModifyMoveSpeed(cur_speed_left, 0, true);
				ModifyMoveSpeed(cur_speed_right, 0, false);
				OLED_ShowString(3, 1, "stop");
				break;
			}
		}
	}
	
	else if(RIGHT_BACKWARD == way)
	{
		ModifyMoveSpeed(cur_speed_left, -50, true);
		ModifyMoveSpeed(cur_speed_right, 0, false);
		angle = 0;
		while(1)
		{
			Delay_ms(10);
			if((angle <= -90 && RightAngleFlag) || (angle >= 90 && !RightAngleFlag))
			{
				ModifyMoveSpeed(cur_speed_left, 0, true);
				ModifyMoveSpeed(cur_speed_right, 0, false);
				break;
			}
		}
	}
	
	else if(LEFT_BACKWARD == way)
	{
		ModifyMoveSpeed(cur_speed_left, 0, true);
		ModifyMoveSpeed(cur_speed_right, -50, false);
		angle = 0;
		while(1)
		{
//			Delay_ms(10);
//			if((angle >= 90 && RightAngleFlag) || (angle <= -90 && !RightAngleFlag))
			if(angle <= -90)
			{
				ModifyMoveSpeed(cur_speed_left, 0, true);
				ModifyMoveSpeed(cur_speed_right, 0, false);
				OLED_ShowString(3, 1, "stop");
				break;
			}
		}
	}
}


int main(void)
{
//	No_MCU_Sensor sensor;
//	/*模块初始化*/
	OLED_Init();        //OLED初始化
//	Key_Init();            //按键初始化
//	Serial_Init();        //串口初始化
	MPU6050_Init();        //MPU6050初始化
	Timer_Init();        //定时器初始化
//	Encoder1_Init();    //编码器初始化
	Motor_Init();        //步进电机初始化
//	PID_init();            //PID结构体初始化
//	gpio_init();
//	adc_init();
	
//	//得到黑白校准值之后，初始化传感器
//	No_MCU_Ganv_Sensor_Init(&sensor,white,black);
//	const int GetValueFrequency = 8;
//	uint8_t Digtal = 0;
//	TIM_PrescalerConfig(TIM1, 72, TIM_PSCReloadMode_Immediate);
//	TIM_PrescalerConfig(TIM4, 72, TIM_PSCReloadMode_Immediate);
//	Motor_SetSpeed_1(5); //前进	
//	Motor_SetSpeed_4(5);
//	CurCarState = RUN; // 切换为正常运行状态
//	
//	bool GetRxFlag = false;
	

	ModifyMoveWay(LEFT_BACKWARD);
	
	while(1)
	{
		
		

//		//在运行状态实时调整姿态
//		if(CurCarState == RUN)
//		{	
//			Digtal = GetDigtalValue(&sensor, GetValueFrequency);
//			//PID算法的用法为形参1越大，就判断为车子越有左转的趋势，相应做出调整。
//			//如果把陀螺仪测得的数据作为参考，那么GZ的值越大，车子越有左转的趋势
//			//如果把巡线传感器的数据作为参考，那么车子有左转趋势时左边全白，数据为1111，而右边至少有1位为0，左减右的值越大，车子越有左转的趋势
//			left_digital = ((Digtal>>3) & 0x01) * 4 + ((Digtal>>2) & 0x01) * 2 + ((Digtal>>1) & 0x01) * 1 + ((Digtal>>0) & 0x01) * 4;
//			right_digital = ((Digtal>>4) & 0x01) * 4 + ((Digtal>>5) & 0x01) * 2 + ((Digtal>>6) & 0x01) * 1 + ((Digtal>>7) & 0x01) * 4;
//			PID_patroling(left_digital - right_digital, &Speed1, &Speed2);
//			Motor_SetSpeed_1(Speed1);
//			Motor_SetSpeed_4(Speed2);
//			
//			int count = 0;
//			for(int i = 0; i < 8; i++)
//				count += ((Digtal >> i) & 0x1);
//			if(count <= 4) // 发送确认，说明此时到了T字形路口
//			{
//				Serial_TxPacket[0] = 1;
//				Serial_SendPacket();
//				
//				Motor_SetSpeed_1(0);	
//				Motor_SetSpeed_4(0);
//				CurCarState = STOP;
//			}
//		}
//	
//		//读取数据，判断是否转弯	
//		if(Serial_GetRxFlag())
//		{
//			if(Serial_RxPacket[0] == 1)//右转
//			{
//				Motor_SetSpeed_1(0);
//				Motor_SetSpeed_4(Speed2);
//				angle = 0;
//				target_angle = -90.0f;
//				CurCarState = STOP;
//			}
//			else if(Serial_RxPacket[0] == 2)//左转
//			{
//				Motor_SetSpeed_1(Speed1); //右轮
//				Motor_SetSpeed_4(0); //左轮
//				angle = 0;
//				target_angle = 90.0f;
//				CurCarState = STOP;
//			}
//			else if(Serial_RxPacket[0] == 0)//前进
//			{
//				Motor_SetSpeed_1(Speed1);
//				Motor_SetSpeed_4(Speed2);
//				CurCarState = RUN;
//			}
//			GetRxFlag = true;
//		}	
//		
//			
//		//停止状态且转弯角度已达目标
//		if(GetRxFlag == true && CurCarState == STOP && abs((int)angle) >= abs((int)target_angle)) // angle和target_angle都会初始化为0，所以在不修改情况下判断一直生效
//		{
//			Motor_SetSpeed_1(Speed1); //前进	
//			Motor_SetSpeed_4(Speed2);
//			CurCarState = RUN; // 切换为正常运行状态
//			GetRxFlag = false;
//		}	
		
//		ModifyMoveWay(LEFT_BACKWARD);
//		Delay_s(2);
//		ModifyMoveWay(STRAIGHT_BACKWARD);
//		Delay_s(2);
		//Delay_s(2);
//		ModifyMoveWay(RIGHT_BACKWARD);
//		Delay_s(2);
//		ModifyMoveWay(LEFT_FORWARD);
//		Delay_s(2);
//		ModifyMoveWay(RIGHT_FORWARD);
//		Delay_s(2);

	}
}








/**
  * 函    数：TIM2中断函数。小车在运动时每隔一段时间就要查看一次当前的速度，以便及时停下行进任务。分频值配置在Timer.h中，此处是0.05秒中断一次
  * 参    数：无
  * 返 回 值：无
  * 注意事项：此函数为中断函数，无需调用，中断触发后自动执行
  */
void TIM2_IRQHandler(void)
{
    if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET && isTIM2_IRQHandler)        //判断是否是TIM2的更新事件触发的中断
    {
			MPU6050_GetData(&GZ);		//获取MPU6050的数据
			OLED_ShowFNum(1, 1, angle);
			OLED_ShowSignedNum(2, 1, GZ, 4);
			isTIM2_IRQHandler = false;
			Speed = Encoder1_Get();
			//对于角度也是一个道理。直走时它能辅助车子不偏航，转弯时更是要根据这个来判断是否转到位了。MPU6050满量程被设置为±2000°/s，对应AD值为-32768~32767，
			//即GZ除32768再乘2000即为以度为单位的角速度。在完全没有动时GZ测出来也为+1~+2，此处对它进行修正。
			angle += (((float)GZ) - GZ_CORRECTION_COEFFICIENT) / 32768.0f * 2000.0f / 10.0f / 2.0f;
			TIM_ClearITPendingBit(TIM2, TIM_IT_Update);            //清除TIM2更新事件的中断标志位
																													//中断标志位必须清除
																													//否则中断将连续不断地触发，导致主程序卡死
			isTIM2_IRQHandler = true;
    }
}

